def callback(msg):
    print(msg)
    pass

import socket
import serial

class MAVLinkReceiverV1:
    def __init__(self, port, callback ,host_baudrate_or_ip=115200):
        """
        port: 数据源，可以是UDP端口号或串口设备路径
        callback: 数据接收回调函数，接收到数据包时会调用此函数
        host_baudrate_or_ip: 数据源的波特率或者UDP的IP地址(例如'192.168.1.100'或者115200)
        """
        self.callback = callback
        self.buffer = bytearray()
        
        # 判断port是UDP端口还是串口
        if isinstance(port, int):
            # 假设port是UDP端口
            self.host = host_baudrate_or_ip
            self.port = port
            self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            self.sock.bind((self.host, self.port))
        elif isinstance(port, str):
            # 假设port是串口设备路径
            self.ser = serial.Serial(port, baudrate=int(host_baudrate_or_ip), timeout=1)
        else:
            raise ValueError("Port must be an integer (UDP) or a string (serial port)")

    def start_receiving(self):
        """开始接收数据"""
        while True:
            try:
                if hasattr(self, 'sock'):
                    # 从UDP套接字接收数据
                    data, _ = self.sock.recvfrom(1024)  # 一次读取1024字节
                elif hasattr(self, 'ser'):
                    # 从串口接收数据
                    data = self.ser.read(1024)  # 一次读取1024字节
                else:
                    raise Exception("No valid data source configured")

                self.buffer.extend(data)  # 将接收到的数据添加到缓冲区
                self.process_buffer()
            except KeyboardInterrupt:
                print("接收到中断信号，停止接收数据")
                break
            except Exception as e:
                print(f"接收数据时发生错误: {e}")
                break

    def process_buffer(self):
        """处理缓冲区中的数据"""
        while len(self.buffer) > 1:  # 至少需要两个字节来解析长度
            if self.buffer[0] == 0xfe:  # 检查起始字节
                length = self.buffer[1]  # 获取载荷长度
                if len(self.buffer) >= length + 8:  # 包括起始字节、载荷长度、校验和等
                    # 提取完整的数据包
                    packet = self.buffer[:length + 8]
                    # 调用回调函数
                    self.callback(packet)
                    # 移除已处理的数据包
                    self.buffer = self.buffer[length + 8:]
                else:
                    # 数据不完整，保留缓冲区中的数据
                    break
            else:
                # 起始字节不正确，移除第一个字节并继续检查
                self.buffer = self.buffer[1:]

# 使用示例
if __name__ == "__main__":
    def callback(packet):
        print(f"Received MAVLink packet: {packet}")
        pass

    # 假设数据源是UDP连接，端口为14550
    port_udp = 14550
    ip = '192.168.1.100'
    receiver_udp = MAVLinkReceiverV1(port_udp, callback, host_baudrate_or_ip=ip)
    receiver_udp.start_receiving()

    # 或者假设数据源是串口，设备路径为'/dev/ttyUSB0'
    port_serial = '/dev/ttyUSB0'
    baudrate = 115200
    receiver_serial = MAVLinkReceiverV1(port_serial, callback, host_baudrate_or_ip=baudrate)
    receiver_serial.start_receiving()

